#include <cstdio>
#include <cassert>
#include "boundingbox.h"

// Included files for OpenGL Rendering
#ifdef __APPLE__
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>
#include <GLUT/glut.h>
#else
#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>
#endif

#include "mesh.h"
#include "edge.h"
#include "vertex.h"
#include "triangle.h"
#include "glCanvas.h"
#include<map>
#include<iostream>
#include<vector>
#include<algorithm>

using namespace std;

int Triangle::next_triangle_id = 0;
void insertNormal(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, const Vec3f &p4);
void DrawSpring(const Vec3f &a_o, const Vec3f &b_o, const Vec3f &a, const Vec3f &b, float correction);
Vec3f getNormal(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, const Vec3f &p4);

float AreaOfTriangle(Vec3f p, Vec3f p1, Vec3f p2)
{
	float a,b,c,s,area;
	a = sqrt((p1.x() - p.x())*(p1.x() - p.x()) + (p1.y() - p.y())*(p1.y() - p.y()));
	b = sqrt((p2.x() - p1.x())*(p2.x() - p1.x()) + (p2.y() - p1.y())*(p2.y() - p1.y()));
	c = sqrt((p2.x() - p.x())*(p2.x() - p.x()) + (p2.y() - p.y())*(p2.y() - p.y()));
	s = 0.5*( a + b + c );
	area = sqrt( s*(s-a)*(s-b)*(s-c) );
	return area;
}

Mesh::Mesh(ArgParser *_args) 
{
  args =_args;
  num_d=0;

  // open the file
  //ifstream istr(args->cloth_file.c_str());
  //assert (istr != NULL);
  //string token;

  // read in the simulation parameters
  
  /*istr >> token >> k_structural; assert (token == "k_structural");  // (units == N/m)  (N = kg*m/s^2)
  istr >> token >> k_shear; assert (token == "k_shear");
  istr >> token >> k_bend; assert (token == "k_bend");
  istr >> token >> damping; assert (token == "damping");*/
  k_structural = 1;
  k_shear = 1;
  k_bend = 1;
  damping = 0.05;
  
  // NOTE: correction factor == .1, means springs shouldn't stretch more than 10%
  //       correction factor == 100, means don't do any correction
  
  /*istr >> token >> provot_structural_correction; assert (token == "provot_structural_correction");
  istr >> token >> provot_shear_correction; assert (token == "provot_shear_correction");*/
  provot_structural_correction = 0.1;
  provot_shear_correction = 0.1;

  // the cloth dimensions
  
  //istr >> token >> nx >> ny; // (units == meters)
  nx=17;
  ny=17;
  
  //assert (token == "m");
  //assert (nx >= 2 && ny >= 2);

  // the corners of the cloth
  
  Vec3f a,b,c,d;
  a=Vec3f(0,1,0);
  b=Vec3f(1,1,0);
  c=Vec3f(1,0,0);
  d=Vec3f(0,0,0);

  /*istr >> token >> a; assert (token == "p");
  istr >> token >> b; assert (token == "p");
  istr >> token >> c; assert (token == "p");
  istr >> token >> d; assert (token == "p");*/

  // fabric weight  (units == kg/m^2)
  // denim ~300 g/m^2
  // silk ~70 g/m^2
  float fabric_weight;
  //istr >> token >> fabric_weight; assert (token == "fabric_weight");
  fabric_weight = 0.1;
  
  float area = AreaOfTriangle(a,b,c) + AreaOfTriangle(a,c,d);
  
  // create the particles
  particles = new ClothParticle[nx*ny];
  float mass = area*fabric_weight / float(nx*ny);
  for (int i = 0; i < nx; i++) 
  {
    float x = i/float(nx-1);
    Vec3f ab = (1-x)*a + x*b;
    Vec3f dc = (1-x)*d + x*c;
    for (int j = 0; j < ny; j++) 
	{
      float y = j/float(ny-1);
      ClothParticle &p = getParticle(i,j);
      Vec3f abdc = (1-y)*ab + y*dc;
      p.setOriginalPosition(abdc);
      p.setPosition(abdc);
      p.setVelocity(Vec3f(0,0,0));
      p.setMass(mass);
      p.setFixed(false);
    }
  }

  // the fixed particles
  
  /*while (istr >> token) 
  {
    assert (token == "f");
    int i,j;
    float x,y,z;
    istr >> i >> j >> x >> y >> z;
    ClothParticle &p = getParticle(i,j);
    p.setPosition(Vec3f(x,y,z));
    p.setFixed(true);
  }*/
  
  //Set spring relationships
  //
	for(int ii=0; ii<nx; ii++)
	{
		for(int jj=0; jj<ny; jj++)
		{
			ClothParticle &p = getParticle(ii,jj);
			p.cut=false;
			//cout<<ii<<" "<<jj<<":"<<endl;
			if(ii+1<nx) //struct -
			{
				ClothParticle &p = getParticle(ii,jj);
				ClothParticle &p2 = getParticle(ii+1,jj);
				p.struc.push_back(make_pair(ii+1,jj));//p2);
				p2.struc.push_back(make_pair(ii,jj));//p);
				//cout<<" "<<p2.getPosition()<<endl;
			}
			if(jj+1<ny) //struct |
			{
				ClothParticle &p = getParticle(ii,jj);
				ClothParticle &p2 = getParticle(ii,jj+1);
				p.struc.push_back(make_pair(ii,jj+1));//p2);
				p2.struc.push_back(make_pair(ii,jj));
				//cout<<" "<<p2.getPosition()<<endl;
			}
			
			if( (jj+1<ny) && (ii+1<nx) ) //shear 
			{
				ClothParticle &p = getParticle(ii,jj);
				ClothParticle &p2 = getParticle(ii+1,jj+1);
				p.shear.push_back(make_pair(ii+1,jj+1));
				p2.shear.push_back(make_pair(ii,jj));
				//cout<<" "<<p2.getPosition()<<endl;
			}
			if( (jj-1>=0) && (ii+1<nx) ) //shear /
			{
				ClothParticle &p = getParticle(ii,jj);
				ClothParticle &p2 = getParticle(ii+1,jj-1);
				p.shear.push_back(make_pair(ii+1,jj-1));
				p2.shear.push_back(make_pair(ii,jj));
				//cout<<" "<<p2.getPosition()<<endl;
			}
			
			if(jj+2<ny) //bend |
			{
				ClothParticle &p = getParticle(ii,jj);
				ClothParticle &p2 = getParticle(ii,jj+2);
				p.bend.push_back(make_pair(ii,jj+2));
				p2.bend.push_back(make_pair(ii,jj));
				//cout<<" "<<p2.getPosition()<<endl;
			}
			if(ii+2<nx) //bend -
			{
				ClothParticle &p = getParticle(ii,jj);
				ClothParticle &p2 = getParticle(ii+2,jj);
				p.bend.push_back(make_pair(ii+2,jj));
				p2.bend.push_back(make_pair(ii,jj));
				//cout<<" "<<p2.getPosition()<<endl;
			}
			
		}
	}
	
  computeBoundingBox();
  
}

void Mesh::computeBoundingBox() {
  box = BoundingBox(getParticle(0,0).getPosition());
  for (int i = 0; i < nx; i++) {
    for (int j = 0; j < ny; j++) {
      box.Extend(getParticle(i,j).getPosition());
    }
  }
  
  int count=0;
  for(unsigned int h=0; h<nx; h++)//h<(nx*ny); h++)
  {
	//particles[h].box=BoundingBox(particles[h].getPosition());
	for(unsigned int k=0; k<ny; k++)
	{
		if( (h+1 < nx) && (k > 0) )
		{
			getParticle(h,k-1).box=BoundingBox(getParticle(h,k-1).getPosition());
			getParticle(h,k-1).box.Extend(getParticle(h+1,k).getPosition());
			getParticle(h+1,k).box=BoundingBox(getParticle(h+1,k).getPosition());
			getParticle(h+1,k).box.Extend(getParticle(h,k-1).getPosition());
			count++;
		}
		//box.extend
	}
  }
  cout<<"count: "<<count<<" "<<nx*ny<<endl;
  
}

void Mesh::Animate() 
{
	computeBoundingBox();
	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	//Now, move the cloth particles appropriately
	for (int i = 0; i < nx; i++) 
	{
		//if(pull_out==true) break;
		for (int j = 0; j < ny; j++) 
		{
			//if(pull_out==true) break;
			ClothParticle &p = getParticle(i,j);
			Vec3f pos=p.getPosition();
			
			Vec3f struc_force=Vec3f(0,0,0);
			Vec3f shear_force=Vec3f(0,0,0);
			Vec3f bend_force=Vec3f(0,0,0);
			
			for(int k=0; k<p.struc.size(); k++)
			{
				ClothParticle &p2=getParticle(p.struc[k].first,p.struc[k].second);
				//if(p2.cut==false)
				//{
				Vec3f a=p2.getPosition();
				Vec3f b=p.getPosition();
				Vec3f c=p2.getOriginalPosition();
				Vec3f d=p.getOriginalPosition();
				
				Vec3f s_pos=Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() );
				Vec3f d_pos=Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
				
				float dist= sqrt( (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z()) );
				float dist2=sqrt( (c.x()-d.x())*(c.x()-d.x()) + (c.y()-d.y())*(c.y()-d.y()) + (c.z()-d.z())*(c.z()-d.z()) );
				
				Vec3f unit_vec=Vec3f( s_pos.x()/dist, s_pos.y()/dist, s_pos.z()/dist );
				float check=abs(dist/dist2);  
				Vec3f internal=k_structural*(s_pos-(dist2*unit_vec));
				struc_force=struc_force+internal;//+s_pos;
				
				if( provot_structural_correction*dist2 > abs(dist-dist2) && p2.isFixed()==true)  //( (check > (provot_structural_correction+1)) && p2.isFixed()==true )
				{
					Vec3f spot_p=b;
					Vec3f spot_p2=a;
					
					int count=2;
					Vec3f n_loc=p.getPosition() + (unit_vec*(dist*.005));
					dist= sqrt( (a.x()-n_loc.x())*(a.x()-n_loc.x()) + (a.y()-n_loc.y())*(a.y()-n_loc.y()) + (a.z()-n_loc.z())*(a.z()-n_loc.z()) );
					check=abs(dist/dist2);
					
					while( (check > (provot_structural_correction+1) ) && count<5)
					{
						n_loc=p.getPosition() + (unit_vec*(dist*.005*count));
						dist= sqrt( (a.x()-n_loc.x())*(a.x()-n_loc.x()) + (a.y()-n_loc.y())*(a.y()-n_loc.y()) + (a.z()-n_loc.z())*(a.z()-n_loc.z()) );
						check=abs(dist/dist2);
						//count++;
					}
					p.setPosition(n_loc);
				}
				else if( provot_structural_correction*dist2 > abs(dist-dist2) && p.isFixed()==false)
				{
					Vec3f spot_p=b;
					Vec3f spot_p2=a;
					Vec3f s_pos2=( Vec3f( b.x()-a.x(), b.y()-a.y(), b.z()-a.z() ) );
					Vec3f unit_vec2=Vec3f( s_pos2.x()/dist, s_pos2.y()/dist, s_pos2.z()/dist );
					
					int count=2;
					Vec3f n_loc=p.getPosition() + (unit_vec*(dist*.0005));
					Vec3f n_loc2=p2.getPosition() + (unit_vec2*(dist*.0005));
					
					dist= sqrt( (n_loc2.x()-n_loc.x())*(n_loc2.x()-n_loc.x()) + (n_loc2.y()-n_loc.y())*(n_loc2.y()-n_loc.y()) + (n_loc2.z()-n_loc.z())*(n_loc2.z()-n_loc.z()) );
					check=abs(dist/dist2);
					
					while( (check > (provot_structural_correction+1) ) && count<5)
					{
						n_loc=p.getPosition() + (unit_vec*(dist*.0005*count));
						n_loc2=p2.getPosition() + (unit_vec2*(dist*.0005*count));
						dist= sqrt( (n_loc2.x()-n_loc.x())*(n_loc2.x()-n_loc.x()) + (n_loc2.y()-n_loc.y())*(n_loc2.y()-n_loc.y()) + (n_loc2.z()-n_loc.z())*(n_loc2.z()-n_loc.z()) );
						check=abs(dist/dist2);
						count++;
					}
					p.setPosition(n_loc);
				}	
				//}
				
			}
			for(int k=0; k<p.shear.size(); k++)
			{
				ClothParticle &p2=getParticle(p.shear[k].first,p.shear[k].second);
				//if(p2.cut==false)
				//{
				Vec3f a=p2.getPosition();
				Vec3f b=p.getPosition();
				Vec3f c=p2.getOriginalPosition();
				Vec3f d=p.getOriginalPosition();
				Vec3f s_pos=( Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() ) );
				Vec3f d_pos=Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
				
				float dist= sqrt( (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z()) );
				float dist2=sqrt( (c.x()-d.x())*(c.x()-d.x()) + (c.y()-d.y())*(c.y()-d.y()) + (c.z()-d.z())*(c.z()-d.z()) );
				
				Vec3f unit_vec=Vec3f( s_pos.x()/dist, s_pos.y()/dist, s_pos.z()/dist );
				float check=abs(dist/dist2);
				Vec3f internal=k_structural*(s_pos-(dist2*unit_vec));
				shear_force=shear_force+internal;		
				
				if( provot_shear_correction*dist2 > abs(dist-dist2) && p2.isFixed()==true)      //( (check > (provot_shear_correction+1)) && p2.isFixed()==true && p.isFixed()==false)
				{
					Vec3f spot_p=b;
					Vec3f spot_p2=a;
					
					int count=2;
					Vec3f n_loc=p.getPosition() + (unit_vec*(dist*.005));
					dist= sqrt( (a.x()-n_loc.x())*(a.x()-n_loc.x()) + (a.y()-n_loc.y())*(a.y()-n_loc.y()) + (a.z()-n_loc.z())*(a.z()-n_loc.z()) );
					check=abs(dist/dist2);
					
					while( (check > (provot_structural_correction+1) ) && count<5)
					{
						n_loc=p.getPosition() + (unit_vec*(dist*.005*count));
						dist= sqrt( (a.x()-n_loc.x())*(a.x()-n_loc.x()) + (a.y()-n_loc.y())*(a.y()-n_loc.y()) + (a.z()-n_loc.z())*(a.z()-n_loc.z()) );
						check=abs(dist/dist2);
						//count++;
					}
					p.setPosition(n_loc);
					
				}
				else if( provot_shear_correction*dist2 > abs(dist-dist2) && p.isFixed()==false)
				{
					Vec3f spot_p=b;
					Vec3f spot_p2=a;
					Vec3f s_pos2=( Vec3f( b.x()-a.x(), b.y()-a.y(), b.z()-a.z() ) );
					Vec3f unit_vec2=Vec3f( s_pos2.x()/dist, s_pos2.y()/dist, s_pos2.z()/dist );
					
					int count=2;
					Vec3f n_loc=p.getPosition() + (unit_vec*(dist*.0005));
					Vec3f n_loc2=p2.getPosition() + (unit_vec2*(dist*.0005));
					
					dist= sqrt( (n_loc2.x()-n_loc.x())*(n_loc2.x()-n_loc.x()) + (n_loc2.y()-n_loc.y())*(n_loc2.y()-n_loc.y()) + (n_loc2.z()-n_loc.z())*(n_loc2.z()-n_loc.z()) );
					check=abs(dist/dist2);
					
					while( (check > (provot_structural_correction+1) ) && count<5)
					{
						n_loc=p.getPosition() + (unit_vec*(dist*.0005*count));
						n_loc2=p2.getPosition() + (unit_vec2*(dist*.0005*count));
						dist= sqrt( (n_loc2.x()-n_loc.x())*(n_loc2.x()-n_loc.x()) + (n_loc2.y()-n_loc.y())*(n_loc2.y()-n_loc.y()) + (n_loc2.z()-n_loc.z())*(n_loc2.z()-n_loc.z()) );
						check=abs(dist/dist2);
						count++;
					}
					p.setPosition(n_loc);
					 
				}
				//}				
				
			}
			for(int k=0; k<p.bend.size(); k++)
			{
				ClothParticle &p2=getParticle(p.bend[k].first,p.bend[k].second);
				//if(p2.cut==false)
				//{
					Vec3f a=p2.getPosition();
					Vec3f b=p.getPosition();
					Vec3f c=p2.getOriginalPosition();
					Vec3f d=p.getOriginalPosition();
					Vec3f s_pos=( Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() ) );
					Vec3f d_pos=Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
					
					float dist= sqrt( (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z()) ); //current length
					float dist2=sqrt( (c.x()-d.x())*(c.x()-d.x()) + (c.y()-d.y())*(c.y()-d.y()) + (c.z()-d.z())*(c.z()-d.z()) ); //original length
					
					Vec3f unit_vec=Vec3f( s_pos.x()/dist, s_pos.y()/dist, s_pos.z()/dist );
					Vec3f internal=k_structural*(s_pos-(dist2*unit_vec));
					bend_force=bend_force+internal;
				//}
			}
			
			if(p.isFixed()==false)
			{
				Vec3f gravity_force=args->gravity*p.getMass();
				
				/*if( (i == (j+10)) && i>10)
				{
					if(j+10 < ny && i+1 < nx)
					{
						const ClothParticle &pp=getParticle(i,j);
						const ClothParticle &pp2=getParticle(i+1,j+10);
						Vec3f a=pp2.getPosition();
						Vec3f b=pp.getPosition();
						Vec3f slice_force=Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() );
						float dist= sqrt( (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z()) );
						Vec3f unit_vec=Vec3f( slice_force.x()/dist, slice_force.y()/dist, slice_force.z()/dist );
						slice_force=unit_vec*1.01;
						gravity_force=gravity_force+slice_force;
					}
			
				}*/
				
				Vec3f accel = struc_force+shear_force+bend_force+gravity_force;
				float mag= sqrt( (accel.x()*accel.x()) + (accel.y()*accel.y()) + (accel.z()*accel.z()) );
				
				if(mag > .08)
				{
					//cout<<mag<<endl;
					//p.cut=true;
					//p.struc.empty();
					//p.shear.empty();
					//p.bend.empty();
				}
				
				accel=Vec3f(accel.x()/p.getMass(),accel.y()/p.getMass(),accel.z()/p.getMass());
				
				//p.setAcceleration(accel);//*args->timestep);
				
				Vec3f Vi=p.getVelocity();
				//Vec3f Vf=p.getVelocity()+ ( (p.getAcceleration()) * args->timestep);
				Vec3f Vf=p.getVelocity()+args->timestep*( (p.getAcceleration() + accel) )*(0.5);
				
				p.setAcceleration(accel);//*args->timestep);
				
				Vf=Vf*(1-damping);
				Vec3f Vif=Vi+Vf;
				
				Vec3f n_pos=(p.getPosition() + Vf*args->timestep);
				Vec3f pos=p.getPosition();
				
				//Adjust new position and new velocity accordingly
				p.setPosition( p.getPosition() + (args->timestep)*(Vi) + ((args->timestep)*(args->timestep)*(p.getPosition())*(0.5)) );
				
				//p.setPosition( p.getPosition() + Vf*args->timestep );  //( ( Vec3f(Vif.x()/2,Vif.y()/2,Vif.z()/2) )*args->timestep) ); 	
				p.setVelocity(Vf);
			}
		}
	}
}

// =======================================================================
// MESH DESTRUCTOR 
// =======================================================================

Mesh::~Mesh() {
  // delete all the triangles
  std::vector<Triangle*> todo;
  for (triangleshashtype::iterator iter = triangles.begin();
       iter != triangles.end(); iter++) {
    Triangle *t = iter->second;
    todo.push_back(t);
  }
  int num_triangles = todo.size();
  for (int i = 0; i < num_triangles; i++) {
    removeTriangle(todo[i]);
  }
  // delete all the vertices
  int num_vertices = numVertices();
  for (int i = 0; i < num_vertices; i++) {
    delete vertices[i];
  }
}

// =======================================================================
// MODIFIERS:   ADD & REMOVE
// =======================================================================

Vertex* Mesh::addVertex(const Vec3f &position) {
  int index = numVertices();
  Vertex *v = new Vertex(index, position);
  vertices.push_back(v);
  if (numVertices() == 1)
    bbox = BoundingBox(position,position);
  else 
    bbox.Extend(position);
  return v;
}


Triangle* Mesh::addTriangle(Vertex *a, Vertex *b, Vertex *c) {
  // create the triangle
  Triangle *t = new Triangle();
  // create the edges
  Edge *ea = new Edge(a,b,t);
  Edge *eb = new Edge(b,c,t);
  Edge *ec = new Edge(c,a,t);
  // point the triangle to one of its edges
  t->setEdge(ea);
  // connect the edges to each other
  ea->setNext(eb);
  eb->setNext(ec);
  ec->setNext(ea);
  // verify these edges aren't already in the mesh 
  // (which would be a bug, or a non-manifold mesh)
  //assert (edges.find(std::make_pair(a,b)) == edges.end());
  //assert (edges.find(std::make_pair(b,c)) == edges.end());
  //assert (edges.find(std::make_pair(c,a)) == edges.end());
  // add the edges to the master list
  edges[std::make_pair(a,b)] = ea;
  edges[std::make_pair(b,c)] = eb;
  edges[std::make_pair(c,a)] = ec;
  // connect up with opposite edges (if they exist)
  edgeshashtype::iterator ea_op = edges.find(std::make_pair(b,a)); 
  edgeshashtype::iterator eb_op = edges.find(std::make_pair(c,b)); 
  edgeshashtype::iterator ec_op = edges.find(std::make_pair(a,c)); 
  if (ea_op != edges.end()) { ea_op->second->setOpposite(ea); }
  if (eb_op != edges.end()) { eb_op->second->setOpposite(eb); }
  if (ec_op != edges.end()) { ec_op->second->setOpposite(ec); }
  // add the triangle to the master list
  assert (triangles.find(t->getID()) == triangles.end());
  triangles[t->getID()] = t;
  
  return t;
}


void Mesh::removeTriangle(Triangle *t) {
  Edge *ea = t->getEdge();
  Edge *eb = ea->getNext();
  Edge *ec = eb->getNext();
  Vertex *a = ea->getStartVertex();
  Vertex *b = eb->getStartVertex();
  Vertex *c = ec->getStartVertex();
  // remove these elements from master lists
  edges.erase(std::make_pair(a,b)); 
  edges.erase(std::make_pair(b,c)); 
  edges.erase(std::make_pair(c,a)); 
  triangles.erase(t->getID());
  // clean up memory
  delete ea;
  delete eb;
  delete ec;
  delete t;
}


// =======================================================================
// Helper functions for accessing data in the hash table
// =======================================================================

Edge* Mesh::getMeshEdge(Vertex *a, Vertex *b) const {
  edgeshashtype::const_iterator iter = edges.find(std::make_pair(a,b));
  if (iter == edges.end()) return NULL;
  return iter->second;
}

Vertex* Mesh::getChildVertex(Vertex *p1, Vertex *p2) const {
  vphashtype::const_iterator iter = vertex_parents.find(std::make_pair(p1,p2)); 
  if (iter == vertex_parents.end()) return NULL;
  return iter->second; 
}

void Mesh::setParentsChild(Vertex *p1, Vertex *p2, Vertex *child) {
  assert (vertex_parents.find(std::make_pair(p1,p2)) == vertex_parents.end());
  vertex_parents[std::make_pair(p1,p2)] = child; 
}


// =======================================================================
// the load function parses very simple .obj files
// the basic format has been extended to allow the specification 
// of crease weights on the edges.
// =======================================================================

void Mesh::Load(const char *input_file) {
  
  FILE *objfile = fopen(input_file,"r");
  if (objfile == NULL) {
    printf ("ERROR! CANNOT OPEN '%s'\n",input_file);
    return;
  }

  char line[200];
  char token[100];
  char atoken[100];
  char btoken[100];
  char ctoken[100];
  float x,y,z;
  int a,b,c,d,e;
  
  int index = 0;
  int vert_count = 0;
  int vert_index = 1;
  
  while (fgets(line, 200, objfile)) {   
    int token_count = sscanf (line, "%s\n",token);
    if (token_count == -1) continue;
    a = b = c = d = e = -1;
    if (!strcmp(token,"usemtl") ||
	!strcmp(token,"g")) {
      vert_index = 1; 
      index++;
    } else if (!strcmp(token,"v")) {
      vert_count++;
      sscanf (line, "%s %f %f %f\n",token,&x,&y,&z);
      addVertex(Vec3f(x,y,z));
    } else if (!strcmp(token,"f")) {
      int num = sscanf (line, "%s %s %s %s\n",token,
			atoken,btoken,ctoken);
      sscanf (atoken,"%d",&a);
      sscanf (btoken,"%d",&b);
      sscanf (ctoken,"%d",&c);
      assert (num == 4);
      a -= vert_index;
      b -= vert_index;
      c -= vert_index;
      assert (a >= 0 && a < numVertices());
      assert (b >= 0 && b < numVertices());
      assert (c >= 0 && c < numVertices());
      addTriangle(getVertex(a),getVertex(b),getVertex(c)); 
    } else if (!strcmp(token,"e")) {
      int num = sscanf (line, "%s %s %s %s\n",token,atoken,btoken,ctoken);
      assert (num == 4);
      sscanf (atoken,"%d",&a);
      sscanf (btoken,"%d",&b);
      if (!strcmp(ctoken,"inf")) x = 1000000; // this is close to infinity...
      else sscanf (ctoken,"%f",&x);
      Vertex *va = getVertex(a);
      Vertex *vb = getVertex(b);
      Edge *ab = getMeshEdge(va,vb);
      Edge *ba = getMeshEdge(vb,va);
      assert (ab != NULL);
      assert (ba != NULL);
      ab->setCrease(x);
      ba->setCrease(x);
    } else if (!strcmp(token,"vt")) {
    } else if (!strcmp(token,"vn")) {
    } else if (token[0] == '#') {
    } else {
      printf ("LINE: '%s'",line);
    }
  }
}


// =======================================================================
// PAINT
// =======================================================================

Vec3f ComputeNormal(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3) {
  Vec3f v12 = p2;
  v12 -= p1;
  Vec3f v23 = p3;
  v23 -= p2;
  Vec3f normal;
  Vec3f::Cross3(normal,v12,v23);
  normal.Normalize();
  return normal;
}

void InsertNormal(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3) {
  Vec3f normal = ComputeNormal(p1,p2,p3);
  glNormal3f(normal.x(), normal.y(), normal.z());
}

void Mesh::Paint2()
{	
	// =====================================================================================
  // render the particles
  // =====================================================================================

  if (args->particles) {
    glDisable(GL_LIGHTING);
    glPointSize(3);
    glBegin(GL_POINTS);
    for (int i = 0; i < nx; i++) {
      for (int j = 0; j < ny; j++) {
	const ClothParticle &p = getParticle(i,j);
	const Vec3f &pos = p.getPosition();
	bool fixed = p.isFixed();
	if (fixed) 
	  glColor3f(0,1,0);
	else
	  glColor3f(0,0,0);	
	glVertex3f(pos.x(),pos.y(),pos.z());
      }
    }
    glEnd();
    glEnable(GL_LIGHTING);
  } 

  // =====================================================================================
  // render the velocity at each particle
  // =====================================================================================

  if (args->velocity) {
    glDisable(GL_LIGHTING);
    glLineWidth(2);
    glBegin(GL_LINES);
    glColor3f(1,0,0);
    for (int i = 0; i < nx; i++) {
      for (int j = 0; j < ny; j++) {
        const ClothParticle &p = getParticle(i,j);
        const Vec3f &pos = p.getPosition();
        const Vec3f &vel = p.getVelocity();
        Vec3f v = pos + vel;
        glVertex3f(pos.x(),pos.y(),pos.z());
        glVertex3f(v.x(),v.y(),v.z());
      }
    }
    glEnd();
    glEnable(GL_LIGHTING);
  } 

  // =====================================================================================
  // render the force at each particle
  // =====================================================================================

  if (args->force) {
    glDisable(GL_LIGHTING);
    glLineWidth(2);
    glBegin(GL_LINES);
    glColor3f(0,0,1);
    for (int i = 0; i < nx; i++) 
	{
      for (int j = 0; j < ny; j++) 
	  {
        const ClothParticle &p = getParticle(i,j);
		const Vec3f &pos = p.getPosition();
		Vec3f struc_force=Vec3f(0,0,0);
		Vec3f shear_force=Vec3f(0,0,0);
		Vec3f bend_force=Vec3f(0,0,0);
		for(int k=0; k<p.struc.size(); k++)
		{
			const ClothParticle &p2=getParticle(p.struc[k].first,p.struc[k].second);
			Vec3f a=p2.getPosition();
			Vec3f b=p.getPosition();
			Vec3f c=p2.getOriginalPosition();
			Vec3f d=p.getOriginalPosition();
			Vec3f s_pos=Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() );
			
			
			Vec3f d_pos=Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
			
			float dist= sqrt( (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z()) );
			float dist2=sqrt( (c.x()-d.x())*(c.x()-d.x()) + (c.y()-d.y())*(c.y()-d.y()) + (c.z()-d.z())*(c.z()-d.z()) );
			
			float diff=(dist-dist2);
			
			Vec3f unit_vec=Vec3f( s_pos.x()/dist, s_pos.y()/dist, s_pos.z()/dist );
			unit_vec=unit_vec*diff;
			s_pos=unit_vec*k_structural;
			
			glVertex3f(pos.x(),pos.y(),pos.z());
			glVertex3f(pos.x()+s_pos.x(),pos.y()+s_pos.y(),pos.z()+s_pos.z());
			
		}
		for(int k=0; k<p.shear.size(); k++)
		{
			const ClothParticle &p2=getParticle(p.shear[k].first,p.shear[k].second);
			Vec3f a=p2.getPosition();
			Vec3f b=p.getPosition();
			Vec3f c=p2.getOriginalPosition();
			Vec3f d=p.getOriginalPosition();
			Vec3f s_pos=Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() );//-a; //Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() ) - Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
			//s_pos=s_pos*k_shear;//*p.getMass();
			
			Vec3f d_pos=Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
			
			float dist= sqrt( (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z()) );
			float dist2=sqrt( (c.x()-d.x())*(c.x()-d.x()) + (c.y()-d.y())*(c.y()-d.y()) + (c.z()-d.z())*(c.z()-d.z()) );
			
			float diff=(dist-dist2);
			
			Vec3f unit_vec=Vec3f( s_pos.x()/dist, s_pos.y()/dist, s_pos.z()/dist );
			unit_vec=unit_vec*diff;
			s_pos=unit_vec*k_shear;
			
			glVertex3f(pos.x(),pos.y(),pos.z());
			glVertex3f(pos.x()+s_pos.x(),pos.y()+s_pos.y(),pos.z()+s_pos.z());
			
		}
		for(int k=0; k<p.bend.size(); k++)
		{
			const ClothParticle &p2=getParticle(p.bend[k].first,p.bend[k].second);
			Vec3f a=p2.getPosition();
			Vec3f b=p.getPosition();
			Vec3f c=p2.getOriginalPosition();
			Vec3f d=p.getOriginalPosition();
			Vec3f s_pos=Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() );//-a; //Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() ) - Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
			//s_pos=s_pos*k_bend;//*p.getMass();
			
			Vec3f d_pos=Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
			
			float dist= sqrt( (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z()) );
			float dist2=sqrt( (c.x()-d.x())*(c.x()-d.x()) + (c.y()-d.y())*(c.y()-d.y()) + (c.z()-d.z())*(c.z()-d.z()) );
			
			float diff=(dist-dist2);
			
			Vec3f unit_vec=Vec3f( s_pos.x()/dist, s_pos.y()/dist, s_pos.z()/dist );
			unit_vec=unit_vec*diff;
			s_pos=unit_vec*k_bend;
			
			glVertex3f(pos.x(),pos.y(),pos.z());
			glVertex3f(pos.x()+s_pos.x(),pos.y()+s_pos.y(),pos.z()+s_pos.z());
			
		}

		Vec3f gravity_force=args->gravity*p.getMass();
		/*if( (i == (j+10)) && i>10)
		{
			if(j+10 < ny && i+1 < nx)
			{
			const ClothParticle &pp=getParticle(i,j);
			const ClothParticle &pp2=getParticle(i+1,j+10);
			Vec3f a=pp2.getPosition();
			Vec3f b=pp.getPosition();
			Vec3f slice_force=Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() );
			float dist= sqrt( (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z()) );
			Vec3f unit_vec=Vec3f( slice_force.x()/dist, slice_force.y()/dist, slice_force.z()/dist );
			slice_force=unit_vec*1.01;
			gravity_force=gravity_force+slice_force;
			}
			
		}*/
		glVertex3f(pos.x(),pos.y(),pos.z());      
		glVertex3f(pos.x()+gravity_force.x(),pos.y()+gravity_force.y(),pos.z()+gravity_force.z());
	
      }
    }
    glEnd();
    glEnable(GL_LIGHTING);
  } 
}

void Mesh::Paint(ArgParser *args) 
{
	Paint2();
  // scale it so it fits in the window
  Vec3f center; bbox.getCenter(center);
  float s = 1/bbox.maxDim();
  glScalef(s,s,s);
  glTranslatef(-center.x(),-center.y(),-center.z());

  // this offset prevents "z-fighting" bewteen the edges and faces
  // the edges will always win.
  glEnable(GL_POLYGON_OFFSET_FILL);
  glPolygonOffset(1.1,4.0);

  glColor3f(1,1,1);
  // draw the triangles
  if (args->gouraud) 
  {
	glBegin (GL_TRIANGLES);
	for (triangleshashtype::iterator iter = triangles.begin(); iter != triangles.end(); iter++) 
	{	
		Triangle *t = iter->second;
		
		Edge *e = t->getEdge();
		Edge *temp=e;
		Vertex *v = e->getStartVertex();
		vector<Vec3f> triangle_normals;
		
		Edge *e_next = e->getNext();
		Edge *temp2=e_next;
		Vertex *v2 = e_next->getStartVertex();
		vector<Vec3f> triangle_normals2;
		
		Edge *e_next_next = e_next->getNext();
		Edge *temp3 = e_next_next;
		Vertex *v3 = e_next_next->getStartVertex();
		vector<Vec3f> triangle_normals3;
		
		Vec3f a = (*t)[0]->getPos();
		Vec3f b = (*t)[1]->getPos();
		Vec3f c = (*t)[2]->getPos();
		Vec3f norm = ComputeNormal(a,b,c);
		triangle_normals.push_back(norm);
		triangle_normals2.push_back(norm);
		triangle_normals3.push_back(norm);
		
		do //look for and store all the triangles around the first point, compute normal of the nearby triangle
		{
			Edge *tempx = ( (temp->getNext())->getOpposite() );
			temp=temp->getNext();
			Triangle *t1 = tempx->getTriangle();
			Vec3f a1 = (*t1)[0]->getPos();
			Vec3f b1 = (*t1)[1]->getPos();
			Vec3f c1 = (*t1)[2]->getPos();
			Vec3f norm1 = ComputeNormal(a1,b1,c1);
			triangle_normals.push_back(norm1);
		}
		while(temp!=e);
		do //look for and store all the triangles around the second point, compute normal of the nearby triangle
		{
			Edge *tempx = ( (temp2->getNext())->getOpposite() );
			temp2=temp2->getNext();
			Triangle *t1 = tempx->getTriangle();
			Vec3f a1 = (*t1)[0]->getPos();
			Vec3f b1 = (*t1)[1]->getPos();
			Vec3f c1 = (*t1)[2]->getPos();
			Vec3f norm1 = ComputeNormal(a1,b1,c1);
			triangle_normals2.push_back(norm1);
		}
		while(temp2!=e_next);
		do //look for and store all the triangles around the third point, compute normal of the nearby triangle
		{
			Edge *tempx = ( (temp3->getNext())->getOpposite() );
			temp3=temp3->getNext();
			Triangle *t1 = tempx->getTriangle();
			Vec3f a1 = (*t1)[0]->getPos();
			Vec3f b1 = (*t1)[1]->getPos();
			Vec3f c1 = (*t1)[2]->getPos();
			Vec3f norm1 = ComputeNormal(a1,b1,c1);
			triangle_normals3.push_back(norm1);
		}
		while(temp3!=e_next_next);
		
		Vec3f average,average2,average3;  //add up all the normals from surrounding triangles
		for(unsigned int i=0; i<triangle_normals.size(); i++)
			average+=triangle_normals[i];
		for(unsigned int i=0; i<triangle_normals2.size(); i++)
			average2+=triangle_normals2[i];
		for(unsigned int i=0; i<triangle_normals3.size(); i++)
			average3+=triangle_normals3[i];
			
		int ii=triangle_normals.size();
		int ii2=triangle_normals2.size();
		int ii3=triangle_normals3.size();
		
		average=Vec3f( (average.x()/ ii),(average.y()/ii),(average.z()/ii) ); //find the average normal
		average2=Vec3f( (average2.x()/ ii2),(average2.y()/ii2),(average2.z()/ii2) );
		average3=Vec3f( (average3.x()/ ii3),(average3.y()/ii3),(average3.z()/ii3) );
		
		v->normal=average; //set normals
		v2->normal=average2;
		v3->normal=average3;
		
	}
	
	for (triangleshashtype::iterator iter = triangles.begin(); iter != triangles.end(); iter++) //apply normals
	{
		Triangle *t = iter->second;
		Vertex *a = (*t)[0];
		Vertex *b = (*t)[1];
		Vertex *c = (*t)[2];
		
		glNormal3f(a->normal.x(),a->normal.y(),a->normal.z());
		glVertex3f(a->x(),a->y(),a->z());
		glNormal3f(b->normal.x(),b->normal.y(),b->normal.z());
		glVertex3f(b->x(),b->y(),b->z());
		glNormal3f(c->normal.x(),c->normal.y(),c->normal.z());
		glVertex3f(c->x(),c->y(),c->z());
		
	}
	
   glEnd();
	
  } 
  else {
    glBegin (GL_TRIANGLES);
    for (triangleshashtype::iterator iter = triangles.begin();
	 iter != triangles.end(); iter++) {
      Triangle *t = iter->second;
      Vec3f a = (*t)[0]->getPos();
      Vec3f b = (*t)[1]->getPos();
      Vec3f c = (*t)[2]->getPos();
      InsertNormal(a,b,c); 
      glVertex3f(a.x(),a.y(),a.z());
      glVertex3f(b.x(),b.y(),b.z());
      glVertex3f(c.x(),c.y(),c.z());
    }
    glEnd();
  }
  glDisable(GL_POLYGON_OFFSET_FILL); 


  // =================================
  if (args->wireframe) {
    glDisable(GL_LIGHTING);

    // draw all the interior, non-crease edges
    glLineWidth(1);
    glColor3f(0,0,0);
    glBegin (GL_LINES);
    for (edgeshashtype::iterator iter = edges.begin();
	 iter != edges.end(); iter++) {
      Edge *e = iter->second;
      if (e->getOpposite() == NULL || e->getCrease() > 0) continue;
      Vec3f a = e->getStartVertex()->getPos();
      Vec3f b = e->getEndVertex()->getPos();
      glVertex3f(a.x(),a.y(),a.z());
      glVertex3f(b.x(),b.y(),b.z());
    }
    glEnd();

    // draw all the interior, crease edges
    glLineWidth(3);
    glColor3f(1,1,0);
    glBegin (GL_LINES);
    for (edgeshashtype::iterator iter = edges.begin();
	 iter != edges.end(); iter++) {
      Edge *e = iter->second;
      if (e->getOpposite() == NULL || e->getCrease() == 0) continue;
      Vec3f a = e->getStartVertex()->getPos();
      Vec3f b = e->getEndVertex()->getPos();
      glVertex3f(a.x(),a.y(),a.z());
      glVertex3f(b.x(),b.y(),b.z());
    }
    glEnd();

    // draw all the boundary edges
    glLineWidth(3);
    glColor3f(1,0,0);
    glBegin (GL_LINES);
    for (edgeshashtype::iterator iter = edges.begin();
	 iter != edges.end(); iter++) {
      Edge *e = iter->second;
      if (e->getOpposite() != NULL) continue;
      assert (e->getCrease() == 0);
      Vec3f a = e->getStartVertex()->getPos();
      Vec3f b = e->getEndVertex()->getPos();
      glVertex3f(a.x(),a.y(),a.z());
      glVertex3f(b.x(),b.y(),b.z());
    }
    glEnd();

    glEnable(GL_LIGHTING);
  }

  HandleGLError(); 
}

struct vec3f_comp {
  bool operator() (const Vec3f& V, const Vec3f& V2) const
  {
	return( sqrt( (V2.x()*V2.x()) + (V2.y()*V2.y()) + (V2.z()*V2.z()) ) < sqrt( (V.x()*V.x()) + (V.y()*V.y()) + (V.z()*V.z()) ) ); 
	}
};


// =================================================================
// SUBDIVISION
// =================================================================

void Mesh::LoopSubdivision() 
{
  printf ("Subdivide the mesh!\n");
  
  map<Vec3f,int,vec3f_comp> vertex_loc; //store vertices based on their locations for easy access
  for(int i=0; i<numVertices(); i++)
  {
	Vertex *a=getVertex(i);
	Vec3f vec=a->getPos();
	vertex_loc[vec]=a->getIndex();
  }
  
  for (triangleshashtype::iterator iter = triangles.begin(); iter != triangles.end(); iter++)
  {
	Triangle *t = iter->second;
	if(t->just_made==0) //don't subdivide triangles that came from the subdivision of a triangle
	{
		Vec3f v1 = t->getEdge()->getStartVertex()->getPos();
		Vec3f v2 = t->getEdge()->getNext()->getStartVertex()->getPos();
		Vec3f v3 = t->getEdge()->getNext()->getNext()->getStartVertex()->getPos();
		
		map<Vec3f,int>::iterator it; //for each vertex, check to see if it's in the map before making a new one
		it=vertex_loc.find(v1);
		Vertex *a;
		if(it!=vertex_loc.end()) 
		{
			a=getVertex(it->second);
		}
		else
		{
			a = (*t)[0];
			vertex_loc[a->getPos()]=a->getIndex();
		}
		
		it=vertex_loc.find(v2);
		Vertex *b;
		if(it!=vertex_loc.end())
		{
			b=getVertex(it->second);
		}
		else
		{
			b = (*t)[1];
			vertex_loc[b->getPos()]=b->getIndex();
		}
		
		it=vertex_loc.find(v3);
		Vertex *c;
		if(it!=vertex_loc.end())
		{
			c=getVertex(it->second);
		}
		else
		{
			c = (*t)[2];
			vertex_loc[c->getPos()]=c->getIndex();
		}
		
			//find the midpoints
		Vec3f a_b = Vec3f( (a->getPos().x()+b->getPos().x())/2, (a->getPos().y()+b->getPos().y())/2, (a->getPos().z()+b->getPos().z())/2 );
		Vec3f b_c = Vec3f( (b->getPos().x()+c->getPos().x())/2, (b->getPos().y()+c->getPos().y())/2, (b->getPos().z()+c->getPos().z())/2 ); 
		Vec3f a_c = Vec3f( (a->getPos().x()+c->getPos().x())/2, (a->getPos().y()+c->getPos().y())/2, (a->getPos().z()+c->getPos().z())/2 ); 
		
		//between a and b
		Vertex *d;
		it=vertex_loc.find(a_b);
		if(it!=vertex_loc.end())
		{
			d=getVertex(it->second);
		}
		else
		{
			d=addVertex(a_b);
			vertex_loc[a_b]=d->getIndex();
		}
			
		//between b and c
		Vertex *e;
		it=vertex_loc.find(b_c);
		if(it!=vertex_loc.end())
		{
			e=getVertex(it->second);
		}
		else
		{
			e=addVertex(b_c);
			vertex_loc[b_c]=e->getIndex();
		}
			
		//between a and c
		Vertex *f;
		it=vertex_loc.find(a_c);
		if(it!=vertex_loc.end())
		{
			f=getVertex(it->second);
		}
		else
		{
			f=addVertex(a_c);
			vertex_loc[a_c]=f->getIndex();
		}
			
		Triangle *tt=addTriangle(a,d,f); //add the triangles
		tt->just_made=1;
		
		Triangle *tt2=addTriangle(b,e,d); 
		tt2->just_made=1;
		
		Triangle *tt3=addTriangle(e,c,f); 
		tt3->just_made=1;
		
		Triangle *tt4=addTriangle(d,e,f);
		tt4->just_made=1;
		
		//tt4->getEdge()->setOpposite(tt2->getEdge()->getNext());
		//tt4->getEdge()->getNext()->setOpposite(tt3->getEdge());
		
		removeTriangle(t);
		//t->just_made=-1;
		iter=triangles.begin();
			
		}	
	}
		
	for (triangleshashtype::iterator iter = triangles.begin(); iter != triangles.end(); iter++) //reset just_made variable to 0 for the next subdivision
	{
		Triangle *t = iter->second;
		t->just_made=0;
	}
	
	vertex_loc.clear(); //clear the map for the next subdivision
}


// =================================================================
// SIMPLIFICATION
// =================================================================

void Mesh::Simplification(int target_tri_count) 
{
  // clear out any previous relationships between vertices
  vertex_parents.clear();

  printf ("Simplify the mesh! %d -> %d\n", numTriangles(), target_tri_count);
  
  for(int i=0; i<numVertices(); i++) //first, find and store the adjacent vertices for each
  {
	if(getVertex(i)->erased==false)
	{
		getVertex(i)->neighbors.clear();
		for(int j=0; j<numVertices(); j++)
		{
			if(j!=i)
			{
				Vertex *a=getVertex(i);
				Vertex *b=getVertex(j);
				Edge *e=getMeshEdge(a,b);
				if(e != NULL) //points a and b for a line
				{
					a->neighbors.push_back(b->getIndex());	//b is adjacent to a, so add it to a's list of neighbors
				}
			}
		}
	}
  }
  map<float, pair<int,int> > legal_edges; //the map of legal edges and the ID nums of their two vertices, map sorts by least edge length to most
  for(int i=0; i<numVertices(); i++)       //find the legal edges by checking each pair of adjacent vertices' neighbors
  {
	if(getVertex(i)->erased==false)
	{
		Vertex *a=getVertex(i);
		for(unsigned int j=0; j<a->neighbors.size(); j++)
		{
			Vertex *b=getVertex(a->neighbors[j]);
			int counter=0;
			for(unsigned int k=0; k<b->neighbors.size(); k++)
			{
				for(unsigned int l=0; l<a->neighbors.size(); l++)
				{
					if(b->neighbors[k]==a->neighbors[l])
						counter++;
				}
			}
			if (counter<=2)
			{
				Edge *e=getMeshEdge(a,b);
				map<float,pair<int,int> >::iterator iter = legal_edges.find(e->Length());
				bool already_there=false;
				while( iter != legal_edges.end() )
				{
					if(iter->second.second==a->getIndex())
					{
						already_there=true;
						break;
					}		
					iter++;
				}
				if(already_there == false && e->Length() != 0) //this is a legal edge, add it's length and vertex ID numbers to the map
					legal_edges[e->Length()]=make_pair(a->getIndex(),b->getIndex());
			}	
		}
	}
  }

  map<float, pair<int,int> >::iterator iter;
  int counter=(int)(numTriangles()*.10);
  for(iter=legal_edges.begin(); iter!=legal_edges.end() && counter>=0; iter++) //go through the list of valid edges, starting with the shortest
  {
	num_d+=1;
	Vertex *a=getVertex(iter->second.first);
	Vertex *b=getVertex(iter->second.second);
	//Edge *e=getMeshEdge(a,b);

	Vec3f pos=a->getPos();
	Vec3f mid = Vec3f( (a->getPos().x()+b->getPos().x())/2, (a->getPos().y()+b->getPos().y())/2, (a->getPos().z()+b->getPos().z())/2 );
	for(int i=0; i<numVertices(); i++) //adjust accordingly
	{
		Vertex *aa=getVertex(i);
		Vec3f aa_pos=aa->getPos();
		if(aa->getIndex() != a->getIndex() && aa_pos == pos)
		{
			aa->setPos(mid);
		}
	}
	b->setPos(mid);
	a->setPos(b->getPos());
	a->erased=true;
	counter--;
  }
  
}

// ================================================================================
// ================================================================================
// some helper drawing functions
// ================================================================================

void DrawSpring(const Vec3f &a_o, const Vec3f &b_o, const Vec3f &a, const Vec3f &b, float correction) {
  Vec3f ab_o = b_o-a_o;
  Vec3f ab = b-a;
  float length_o = ab_o.Length(); // the original length
  float length = ab.Length();     // the current length
  if (length > (1+correction) * length_o ||
      length < (1-correction) * length_o) {
    // draw the spring in cyan if it's over-stretched
    glColor3f(0,1,1);
  } else {
    // otherwise draw it black
    glColor3f(0,0,0);
  }  
  glVertex3f(a.x(),a.y(),a.z());
  glVertex3f(b.x(),b.y(),b.z());
}

Vec3f getNormal(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, const Vec3f &p4)
{
	// compute the normal of a non-planar quadrilateral (not well-defined)
  Vec3f v12 = p2 - p1;
  Vec3f v23 = p3 - p2;
  Vec3f v34 = p4 - p3;
  Vec3f v41 = p1 - p4;

  // compute normals at each corner and average them
  Vec3f normal1, normal2, normal3, normal4;
  Vec3f::Cross3(normal1,v41,v12);
  Vec3f::Cross3(normal2,v12,v23);
  Vec3f::Cross3(normal3,v23,v34);
  Vec3f::Cross3(normal4,v34,v41);
  Vec3f normal = normal1+normal2+normal3+normal4;

  normal.Normalize();
  return normal; 

}

void insertNormal(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, const Vec3f &p4) {
  // compute the normal of a non-planar quadrilateral (not well-defined)
  Vec3f v12 = p2 - p1;
  Vec3f v23 = p3 - p2;
  Vec3f v34 = p4 - p3;
  Vec3f v41 = p1 - p4;

  // compute normals at each corner and average them
  Vec3f normal1, normal2, normal3, normal4;
  Vec3f::Cross3(normal1,v41,v12);
  Vec3f::Cross3(normal2,v12,v23);
  Vec3f::Cross3(normal3,v23,v34);
  Vec3f::Cross3(normal4,v34,v41);
  Vec3f normal = normal1+normal2+normal3+normal4;

  normal.Normalize();
  glNormal3f(normal.x(), normal.y(), normal.z());
  
}

void DrawForce(const Vec3f &p, const Vec3f &f) {
  Vec3f tmp = p+f;
  glVertex3f(p.x(),p.y(),p.z());
  glVertex3f(tmp.x(),tmp.y(),tmp.z());
}

// ================================================================================
// ================================================================================

 